#include "save_pcd.h"
#include <stdio.h>

using namespace cv;
using namespace std;
namespace enc = sensor_msgs::image_encodings;
using namespace cv;




class SavePCDNodeImpl : public SavePCDNode
{
  int frame_num;
  ros::NodeHandle nh_;
  char key;
  char *home;

  ros::Subscriber pointcloud2_sub_;


public:
  SavePCDNodeImpl()
  {
    pointcloud2_sub_ = nh_.subscribe<sensor_msgs::PointCloud2>("point_cloud",1,&SavePCDNodeImpl::pointcloudCb,this);
    frame_num = 0;
    home = getenv("HOME");

  }

  ~SavePCDNodeImpl(){

  }


  virtual void pointcloudCb(const sensor_msgs::PointCloud2ConstPtr &msg)
  {

    if ((msg->width * msg->height) == 0)
      return;

    ROS_INFO ("Received %d data points in frame %s with the following fields:\
 %s", (int)msg->width * msg->height, msg->header.frame_id.c_str (), pcl::\
	      getFieldsList (*msg).c_str ());
    std::cin >> key;
    std::stringstream ss;
    // Check if there is a parameter on the server that sets the prefix for t\
    he output file, 
      std::string prefix;
    nh_.getParam ("prefix", prefix);

    ss << home << "/hironx_data/points_on_plane"<< setw(4) << setfill('0') << frame_num << ".pcd";
    ROS_INFO ("Data saved to %s", ss.str ().c_str ());
    
    pcl::io::savePCDFile (ss.str (), *msg, Eigen::Vector4f::Zero (),
			  Eigen::Quaternionf::Identity (), false);

    frame_num++;


    // if ((msg->width * msg->height) == 0)
    //   return;

    // char filename[128];
    // std::stringstream ss;
    // //ss << "/home/leus/hironx_data/points_on_plane" << frame_num << ".pcd";
    // cout << ss << endl;
      


    // sprintf(filename,"/home/oya/hironx_data/points_on_plane%04d.pcd",frame_num);

    // //pcl::PointCloud<pcl::PointXYZRGB> points;
    // //pcl::PointCloud<pcl::PointXYZ> points;
    // //pcl::fromROSMsg(*msg, points);
    
    // //pcl::io::savePCDFileASCII(filename,points);
    // pcl::io::savePCDFile(filename, *msg, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), true);
    // std::cerr << "Saved " << (int)msg->width * msg->height << " data points to points_on_plane" << std::endl;

  }
  
};

SavePCDNodePtr CreateSavePCDNode()
{
    return SavePCDNodePtr(new SavePCDNodeImpl());
}
